// VideoTool.cpp : Defines the entry point for the console application.

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <tchar.h>
#include <math.h>
#include <vector>
#include <assert.h>

#include <apmatrix.h>
// TODO: reference additional headers your program requires here

#include "cv.h"
#include "highgui.h"

// ARGH! why is this necessary ??
#define _NO_HMC_WINSOCK_
#include "SmallSocket.h"
#undef _NO_HMC_WINSOCK_

using namespace std;

const int MAX_COMPONENTS = 500;
	//Globals here for findCentroid so it doesn't do reallocation each time.
	int numPoints[MAX_COMPONENTS];
	CvPoint sums[MAX_COMPONENTS];
	int componentNums[320][240];

// these are the two functions you'll need to write
void RGBtoHSV( double r, double g, double b, double *h, double *s, double *v );
void processRed( IplImage* frame, IplImage* f_p );
bool isRed(double h, double s, double v, double r, double g, double b);
bool isYellow(double h, double s, double v, double r, double g, double b);
void findBestLine(vector<CvPoint> points, CvPoint* t1, CvPoint* t2);
double distToLine(CvPoint l1, CvPoint l2, CvPoint pt);

void drawMat(IplImage* f_p, int matrix[320][240]);
void dilate(int matrix[320][240], int mainColor);
void erode(int matrix[320][240], int mainColor);
//int safeGetComponentNumber(apmatrix<int> matrix, int x, int y);
int safeGetComponentNumber(int matrix[320][240], int x, int y);

void reconcileComponents(int matrix[320][240], CvPoint sums[320*240], 
						 int numPoints[320*240], int numComponents, int c1, int c2);
void findCentroid(int matrix[320][240], int color, int& x, int& y, int& pixelCount);
CvPoint newCvPoint(int x, int y);


void setGlobals();  // the code appears at the very bottom of this file

char wndname[] = "VideoTool";
char wndname2[] = "Undistorted image";
char wndname3[] = "Painted image";

// here, static refers to the fact that these variables
// are local to this file (they're not available in other files,
// which is safer because it allows reuse of their names)
static CvSize imageSize;
static CvPoint old_click_pt;
static CvPoint new_click_pt;
static double framenum;

// global points for drawing boxes, defining loops, etc.
static CvPoint pt1; 
static CvPoint pt2; 
static CvPoint pt3;
static CvPoint pt4;
static CvPoint pt5;
static CvPoint pt6;
static CvPoint pt7;

// global calibration data
static float* M; 
static float* D;

// more global variables
// these are simply recording all the different states you
// can put the program in via the keyboard
static bool bmpRecording = false;
static bool capturing = true;
static bool getImageFromFile = false;
static bool getImageAgainFromFile = false;
static bool getImage0FromVideoBMPs = false;
static bool saveNextImage = false;
static bool quittingTime = false;
static bool detectRed = false;
static bool showing_savedImage = false;

static char* filename = new char[150];
static char* filename2 = new char[150];
static int frameNumber = 0;
static int keyCode = -1;

// pointers to images in memory
static IplImage *frame;
static IplImage* undis_frame;
static IplImage* undis_frame_painted;
static CvArr* undisMap;

// globals for tuning parameters by keyboard...
static int blueValue = 255;
double hueMin = 330;
double hueMax = 30;
double satMin = .3;
double valMin = .1;

double gThresh = 50;
double bThresh = 50;
double rgRatioMin = 1.5;
double rbRatioMin = 1.5;
bool dilateOn = false;
int closing = 5;
bool testing = false;

static CvPoint centroid;
static pixelColorMin; // min # of colored pixels in range 
static pixelColorMax; // max # of colored pixels in range
static pixelCount;

static int MAX_DIST = 3;
static int MAX_ERROR = 100;

typedef enum
{
	YELLOW = 1
};


// handle mouse clicks here
void mouse_callback(int event, int x, int y, int flags)
{
	if (event == CV_EVENT_LBUTTONDOWN)
	{
		// cout << "(x,y) = (" << x << "," << y << ")" << endl;

		// reset old_click_pt
		old_click_pt.x = new_click_pt.x;
		old_click_pt.y = new_click_pt.y;

		// get new click point -- note the coordinate change in y
		new_click_pt.x = x;  // coming in from the window system
		new_click_pt.y = imageSize.height-y;  // window system and images have different y axes
	}
}

// print pixel information
void pixelInformation(IplImage* f)
{
	if (f == NULL) return;

	// see if things have changed...
	if (new_click_pt.x != old_click_pt.x || new_click_pt.y != old_click_pt.y) // has it changed?
	{
		// draw a line from old to new
		// cvLine(frame,old_click_pt,new_click_pt,CV_RGB(255,0,0),/*thickness*/1,/*connectivity*/8);
		// set the old = the new
		old_click_pt.x = new_click_pt.x;
		old_click_pt.y = new_click_pt.y;
		// print the RGB values and coordinates of the newly clicked point:
		// note that blue is #0, green is #1, and red is #2 !!

		/* note that this uses the undistorted frame right here... */

		// this is insane!!!
		// see the processRed function for an explanation
		// of why we need "realy"
		int realy = new_click_pt.y;
		if (getImageAgainFromFile || getImageFromFile) 
		{
			realy = imageSize.height - realy;
		}
		uchar* pixel = cvPtr2D(f,realy,new_click_pt.x);

		cout << "(x,y) = (" << new_click_pt.x << "," << new_click_pt.y << ") with (r,g,b) = ("
			 << (int)(pixel[2]) << "," << (int)(pixel[1]) << "," << (int)(pixel[0]) << ")";

		double hue,sat,val;
		RGBtoHSV((double)pixel[2],(double)pixel[1],(double)pixel[0],&hue,&sat,&val);

		cout << " and (h,s,v) = (" << hue << "," << sat << "," << val << ")" << endl;
	}
}

// the thread for the server
DWORD WINAPI ThreadFunc( LPVOID lpParam ) 
{ 
   	char recvbuf[80];
	char sendbuf[80];

	string t("HI");
	SmallSocket SS(SmallSocket::SERVER);

    sprintf( recvbuf, "Server Started", *(DWORD*)lpParam ); 
    //MessageBox( NULL, szMsg, "ThreadFunc", MB_OK );
	
	while (true)
	{
		SS.receive(recvbuf,80);
		cout << "Received: " << recvbuf << endl;
		
		if (recvbuf[0] == 'q') {
			SS.sendout("q",2);
			// set a variable that tells it to quit...
			quittingTime = true;
		    // cvReleaseCapture( &capture );
			//	cvDestroyWindow("result");
			break;
		}

		if (recvbuf[0] == 'e') {
			// just an example of passing in more information
			// and using sscanf to extract it
			// sscanf(recvbuf,"q %lf %lf %lf ",&savedXforLog,&savedYforLog,&savedTforLog);
			//
			//the connection requires that something come back...
			// note that we're sending two zeros every time here...
			sprintf(sendbuf, "%d %d", 0, 0);
			SS.sendout(sendbuf,80);
		}

		if (recvbuf[0] == 'c')
		{	// pass x and y values of centroid
			sprintf(sendbuf, "%d %d %d\n", centroid.x, centroid.y, pixelCount);
			SS.sendout(sendbuf,80);
		}

		// pause for a bit...
		Sleep(10);
	}

    return 0; 

} 


int main( int argc, char** argv )
{
	// in case you want to log things...
	//ofstream logfile("logfile_out.txt");
	//ifstream loginfile("logfile_in.txt");

	// set up the globals...
	setGlobals();

	// start the server's thread 
	// make sure that all imageprocessing is in main's thread
	DWORD dwThreadId, dwThrdParam = 1; 
    HANDLE hThread; 
    char szMsg[80];

    hThread = CreateThread( 
        NULL,                        // default security attributes 
        0,                           // use default stack size  
        ThreadFunc,                  // thread function 
        &dwThrdParam,                // argument to thread function 
        0,                           // use default creation flags 
        &dwThreadId);                // returns the thread identifier 
 
    // Check the return value for success. 
    if (hThread == NULL) 
    {
        sprintf( szMsg, "CreateThread failed." ); 
        MessageBox( NULL, szMsg, "main", MB_OK );
    }

	/* 
	 * set up the video windows
	 */
	cvNamedWindow( wndname, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( wndname2, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( wndname3, CV_WINDOW_AUTOSIZE );

	/*
	 * set up the mouse input
	 */
	//cvSetMouseCallback( wndname, mouse_callback );
	cvSetMouseCallback( wndname2, mouse_callback );

	/*
	 * more camera-specific stuff
	 */
	CvCapture* capture = 0;
	capture = cvCaptureFromCAM(0);

	// this is allocating the undis_frame once (it will get written each time)
	// get one frame and then copy it...
	if (cvGrabFrame(capture)) 
	{
		frame = cvRetrieveFrame( capture );
		undis_frame = cvCloneImage(frame);
		undis_frame_painted = cvCloneImage(frame);
		// now let's set up the undistortion parameters
		// note that the docs say 3*wider, but that did not work
		// 3*wider _and_ 3*higher worked...
		undisMap = cvCreateMat( imageSize.width*3, imageSize.height*3, CV_32SC3 );
		// need to release this at the end of the program...
		cvUnDistortInit( frame, undisMap, M, D, 1 );
	}
	else
	{
		cout << "No camera??" << endl;
		capturing = false;
	}

	// main loop
	while (!quittingTime)
    {
		if (capturing)
		{
			if( !cvGrabFrame( capture ))   break;
			frame = cvRetrieveFrame( capture );
			if( !frame ) break;

			// cvUnDistortOnce( frame, undis_frame, M, D, 1 );  // might take some extra time...
			cvUnDistort( frame, undis_frame, undisMap, 1 );
			cvReleaseImage(&undis_frame_painted);
			undis_frame_painted = cvCloneImage(undis_frame);
		}
		else if (getImageFromFile || getImageAgainFromFile)
		{
			if (getImageFromFile) {
				// ask for a frame filename
				int i;
				// should factor this path out!!
				if (getImage0FromVideoBMPs) { 
					i = 0;
					getImage0FromVideoBMPs = false;
				}
				else {
					cout << "What image number (in C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs): ";
					cin >> i;
				}
				sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",i);
				// we probably need to release the images returned by cvLoadImage somewhere...
				//frame = cvLoadImage(filename2);
				//undis_frame = cvCloneImage(frame);
				cvReleaseImage(&undis_frame);
				undis_frame = cvLoadImage(filename2);
				cvReleaseImage(&undis_frame_painted);
				undis_frame_painted = cvLoadImage(filename2);
				if (undis_frame == NULL) {
					cout << "Could not find that image.\n";
					continue;
				}
				getImageFromFile = false;
				getImageAgainFromFile = true;
				
			}
			else if (getImageAgainFromFile) {
				cvReleaseImage(&undis_frame_painted);
				undis_frame_painted = cvCloneImage(undis_frame);
			}
		}
		else
		{
			;
		}

		// click only on undis_image
		pixelInformation(undis_frame_painted);

		// save the images to a file, if desired
		if (bmpRecording == true && capturing == true)
		{
			sprintf(filename,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\BMPs\\image%05d.bmp",frameNumber);
			cout << "Captured single image (dist): " << filename <<  endl;
			cvSaveImage(filename,frame);
			sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\BMPsUnd\\image%05d.bmp",frameNumber);
			cout << "Captured single image (undist): " << filename2 <<  endl;
			cvSaveImage(filename2,undis_frame);
			++frameNumber;
			//saveNextImage = false;
		    //if (getImageFromFile) {
			//	bmpRecording = false; 
			//}
		}

		// here we can add graphics to each frame
		// in order that they show up on the screen
		// we also decide what to display in this routine
		if (detectRed == true)
		{
			processRed(undis_frame,undis_frame_painted);
		}

		/*
		 * display everything in the windows
		 */
		cvShowImage(wndname, frame);
		cvShowImage(wndname2, undis_frame);
		cvShowImage(wndname3, undis_frame_painted);

		//handle key presses
		/*
		 * quits
		 */
		if( (keyCode = cvWaitKey( 10 )) == ((int)'q') ) {          
			break;                       // quit everything
		}
		/*
		 * we use = and - to increase and decrease blueValue
		 */
		else if (keyCode == (int)'=') 
		{
			if (blueValue < 251) blueValue += 5;
		}
		else if (keyCode == (int)'-') 
		{
			if (blueValue > 4) blueValue -= 5;
		}

		else if (keyCode == (int)'1') {
			--gThresh;
		}
		else if (keyCode == (int)'2') {
			++gThresh;
		}
		else if (keyCode == (int)'3') {
			--bThresh;
		}
		else if (keyCode == (int)'4') {
			++bThresh;
		}
		else if (keyCode == (int)'5') {
			rgRatioMin -= .1;
		}
		else if (keyCode == (int)'6') {
			rgRatioMin += .1;
		}
		else if (keyCode == (int)'7') {
			rbRatioMin -= .1;
		}
		else if (keyCode == (int)'8') {
			rbRatioMin += .1;
		}
		else if (keyCode == (int)'D') {
			dilateOn = !dilateOn;
		}
		else if (keyCode == (int)'p') {
			//cout << "gThresh = " << gThresh << ", bThresh = " << bThresh << endl;
			cout << "rgRatioMin = " << rgRatioMin << ", rbRatioMin = " << rbRatioMin
				<< ", Dilate?: " << dilateOn << endl;
		}


		else if (keyCode == (int)'H')
		{
			hueMax++;
			hueMin--;
		}
		else if (keyCode == (int)'h')
		{
			hueMin++;
			hueMax--;
		}
		else if (keyCode == (int)'S')
		{
			satMin += .01;
		}
		else if (keyCode == (int)'s')
		{
			satMin -= .01;
		}
		else if (keyCode == (int)'V')
		{
			valMin += .01;
		}
		else if (keyCode == (int) 'v')
		{
			valMin -= .01;
		}
		else if (keyCode == (int) 'd')
		{
			cout << "Hue ranges: min: " << hueMin << " max: " << hueMax << endl;
			cout << "Saturation min: " << satMin << " value min: " << valMin << endl;
		}
		/*
		 * records video to bitmaps (files)
		 */
		else if (keyCode == (int)'f') {                           
			if (bmpRecording) {
				cout << "Stopping the writing to bitmaps\n";
				bmpRecording = false;
			} else {
				cout << "Starting to write to bitmaps\n";
				bmpRecording = true;
			}
			showing_savedImage = false;
		} 
		/*
		 * gets image from a file # in ./VideoBMPs
		 */
		else if (keyCode == (int)'g') {
			cout << "Getting image from file.\n";
			capturing = false;
			getImageFromFile = true;
			getImageAgainFromFile = false;
			bmpRecording = false;
			saveNextImage = false;
			showing_savedImage = true;
		} 
		/*
		 * toggle continuous video
		 */
		else if (keyCode == (int)'s') {
			if (capturing) {
				cout << "Stopping continuous capturing.\n";
				capturing = false;
				getImageFromFile = false;
			}
			else {
				cout << "Starting continuous capturing.\n";
				capturing = true;
				getImageFromFile = false;
				getImageAgainFromFile = false;
				/* reset undis_image */
				// note this assumes that everything went well
				// initially with the first captured frame... !!
				cvReleaseImage(&undis_frame);
				undis_frame = cvCloneImage(frame);
				cvReleaseImage(&undis_frame_painted);
				undis_frame_painted = cvCloneImage(frame);
				// not to mention that these transition images have to be released!
			}
			showing_savedImage = false;
		} 
		else if (keyCode == (int)'e') {
			// emulate server behavior
			saveNextImage = true;
			showing_savedImage = false;
		} 
		/*
		 * toggle finding red (and displaying)
		 */
		else if (keyCode == (int)'r') {
			detectRed = !detectRed;
		}
		/*
		 * the spacebar advances the image in the VideoBMPs directory
		 */
		else if (keyCode == (int)' ') {
			if (showing_savedImage == true)
			{
				int i;
				sscanf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",&i);
				sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",++i);
				cvReleaseImage(&undis_frame);
				undis_frame = cvLoadImage(filename2);
				cvReleaseImage(&undis_frame_painted);
				undis_frame_painted = cvLoadImage(filename2);

				// wrap around!
				if (undis_frame == NULL) 
				{
					i = 0;
					sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",i);
					cvReleaseImage(&undis_frame);
					undis_frame = cvLoadImage(filename2);
					cvReleaseImage(&undis_frame_painted);
					undis_frame_painted = cvLoadImage(filename2);
					
					continue;
				}
				getImageAgainFromFile = true;
				getImageFromFile = false;
			}
			/*
			 * initial hit of the space bar...
			 */
			else {
				cout << "No image from file is currently showing..." << endl;
				cout << "Getting image 0 from VideoBMPs.\n";
				capturing = false;
				getImageFromFile = true;
				getImageAgainFromFile = false;
				getImage0FromVideoBMPs = true;
				bmpRecording = false;
				saveNextImage = false;
				showing_savedImage = true;
			}
		}
		
	}

	// release things before quitting
    cvReleaseCapture( &capture );
	cvDestroyWindow("result");

} // end of main

// just in case you'd like to use these...
inline double mymin(double a, double b) { return a<b?a:b; }

inline double mymax(double a, double b) { return a>b?a:b; }

// read about HSV at http://en.wikipedia.org/wiki/HSV_color_space
// and http://www.cs.rit.edu/~ncs/color/t_convert.html#RGB%20to%20HSV%20&%20HSV%20to%20RGB
//
// feel free to grab code from above or elsewhere online, if you'd like
// be sure you understand what RANGE _your_ code for HSV is "returning"
// in the h, s, and v pointers...
void RGBtoHSV( double r, double g, double b, double *h, double *s, double *v )
{/*
	double max = (r > g && r > b)? r : (g > b)? g : b;
	double min = (r < g && r < b)? r : (g < b)? g : b;
	double delta = max-min;
	*v = max/255;
	*s = delta/255;
	if(max == r)
		*h = (g - b) / delta;
	else if(max == g)
		*h = 1.0/3 + (b - r) / delta;
	else
		*h = 2.0/3 + (r - g) / delta;
	if (*h < 0)
		*h += 1;
*/

	double delta;
	double max = (r > g && r > b)? r : (g > b)? g : b;
	double min = (r < g && r < b)? r : (g < b)? g : b;



	*v = max / 255.0;				// v

	delta = max - min;

	if( max != 0 )
		*s = delta / max;		// s
	else {
		// r = g = b = 0		// s = 0, v is undefined
		*s = 0;
		*h = -1;
		return;
	}

	if( r == max )
		*h = ( g - b ) / delta;		// between yellow & magenta
	else if( g == max )
		*h = 2 + ( b - r ) / delta;	// between cyan & yellow
	else
		*h = 4 + ( r - g ) / delta;	// between magenta & cyan

	*h *= 60;				// degrees
	if( *h < 0 )
		*h += 360;
}

//Extend the points
void extendLine(CvPoint* p1, CvPoint* p2)
{
	if (p1->x == p2->x)
		return;
	double slope = 0;//(double) (p1->y - p2->y) / (double) (p1->x - p2->x);
	int intercept = (int) (p1->y - slope * p1->x);
	p1->x = (int) mymin(pt4.x, pt5.x);
	p1->y = (int) (intercept + slope * p1->x);
	p2->x = (int) mymax(pt4.x, pt5.x);
	p2->y = (int) (intercept + slope * p2->x);
}

// here is where we seek out the red molding and indicate what we've found
// the input named "frame" is the source location of our pixels
// the "output" named f_p is the destination for drawing
void processRed(IplImage* frame, IplImage* f_p)
{
    // some useful local variables...
	int r,g,b;
	double hue,sat,val;

	// pixels are handled as uchar*'s
	uchar* color;     // a pixel from the input, "frame"
	uchar* painted;   // a pixel from the output, "painted"

	// this draws a rectangle from pt4 to pt5 with the color
	// specified by the three RGB values in the CV_RGB macro
	// the R, G, and B channels are all 0 to 255...

	// see the "this is insane!" caution below in order to
	// use graphics on the live video stream...
	cvRectangle(f_p, pt4, pt5, CV_RGB(0,0,255), 1); // 1 == thickness

	// This loops through the pixels in the rectangle specified
	// by pt4 and pt5 (see setGlobals for their corners)

	int xlow = (int)mymin(pt4.x,pt5.x);
	int xhi = (int)mymax(pt4.x,pt5.x);
	int ylow = (int)mymin(pt4.y,pt5.y);
	int yhi = (int)mymax(pt4.y,pt5.y);

	vector<CvPoint> startPts;
	vector<CvPoint> endPts;
	int pixMat[320][240];

	for (int x = xlow; x<xhi; ++x)		//the x direction goes left to right       
	{
		int distToRed = MAX_DIST;
		CvPoint curTop;
		CvPoint curBot;
		bool foundRed = false;
		vector<int> verticalPixels;
		for (int y = ylow; y<yhi; y++)  //the y direction goes top to bottom
		{
			// this is insane!!!
			// unfortunately, access to the pixels is different depending on
			// the source of the pixels: static images and video reverse their
			// y orientations

			// we handle this here...
			int realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}

			// note that we need to use "realy" in each of these cases
			color = cvPtr2D(frame,realy,x);    // get the pixel we care about
			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			r = (int)(color[2]);  // the red component of the pixel we care about
			g = (int)(color[1]);  // the green component of the pixel we care about
			b = (int)(color[0]);  // the blue component of the pixel we care about

			// you might want to call this here!
			RGBtoHSV(r,g,b,&hue,&sat,&val);

			//Found a red pixel. Now what?
			if (isYellow(hue, sat, val, r, g, b))
			{
				foundRed = true;
				//Start a new red blob
				if (distToRed > MAX_DIST)
				{
					distToRed = 0;
					curTop.x = x;
					curTop.y = y;
				}
				else
				{
					curBot.x = x;
					curBot.y = y;
				}

				pixMat[x][y] = YELLOW;
//				verticalPixels.push_back(1);
			}
			else
			{
				distToRed++;
//				verticalPixels.push_back(0);
				pixMat[x][y] = 0;
			}

			// gratuitous drawing routine, just to show how it works
/*			if ( x%4 != 0 && y%4 != 0 )
			{
				painted[0] = blueValue;  // sets blue to 255
				painted[1] = 0;    // sets green to 0
				painted[2] = 0;    // sets red to 0
			}
*/
		}
		if (foundRed)
		{
			startPts.push_back(curTop);
			endPts.push_back(curBot);
		}
//		pixMat.push_back(verticalPixels);
	}

	//Now starts and ends have all of our start and end points for the red blob. Find the best-fit line for these points
	//CvPoint t1, t2, b1, b2;
	//findBestLine(startPts, &t1, &t2);
	//findBestLine(endPts, &b1, &b2);

	//Make our lines LONG. Because that's the way we like 'em. Long. Yep.
	//extendLine(&t1, &t2);
	//extendLine(&b1, &b2);

	//Draw the lines
	//cvLine(f_p, t1, t2, CV_RGB(0,255,0),1,8);  
	//cvLine(f_p, b1, b2, CV_RGB(0,255,0),1,8);  

	for (int x = xlow; x<xhi; x++)		//the x direction goes left to right       
	{
		for (int y = ylow; y<yhi; y++)  //the y direction goes top to bottom
		{
			// this is insane!!!
			// unfortunately, access to the pixels is different depending on
			// the source of the pixels: static images and video reverse their
			// y orientations

			// we handle this here...
			int realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}

			// note that we need to use "realy" in each of these cases
			color = cvPtr2D(frame,realy,x);    // get the pixel we care about
			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			r = (int)(color[2]);  // the red component of the pixel we care about
			g = (int)(color[1]);  // the green component of the pixel we care about
			b = (int)(color[0]);  // the blue component of the pixel we care about

			// you might want to call this here!
			RGBtoHSV(r,g,b,&hue,&sat,&val);

/*			if (isRed(hue, sat, val, r, g, b))
			{
				painted[0] = 0;
				painted[1] = 1;
				painted[2] = 255;
			}
*/
			/*if(pixMat[x-xlow][y-ylow] == 1)
			{
				painted[0] = 0;
				painted[1] = 1;
				painted[2] = 255;
			}*/

		}// end for y
	}// end for x
	if(dilateOn)
	{
		for(int i = 0; i < closing; i++)
			dilate(pixMat, 1);
		for(int j = 0; j < closing; j++)
			erode(pixMat, 1);
	}
	drawMat(f_p, pixMat);
}

/* Return if the passed-in values qualify as "red"*/
bool isRed(double h, double s, double v, double r, double g, double b)
{
	double rgRatio, rbRatio;
	(g != 0) ? rgRatio = r/g : rgRatio = 1000000;
	(b != 0) ? rbRatio = r/b : rbRatio = 1000000;
	//Whee, red straddling the zero boundary screws everything up
	if ((h < hueMax || h > hueMin) &&
		(s > satMin) && (v > valMin) && 
		(rgRatio > rgRatioMin) && (rbRatio > rbRatioMin))
		//(r-g) > gThresh && (r-b) > bThresh)
		return true;
	return false;
}

/* Return if the passed-in values qualify as "red"*/
bool isYellow(double h, double s, double v, double r, double g, double b)
{
	return (50 < h && h < 70 && s > .5);
}

double sqr(double x)
{
	return x * x;
}

//Find the line that best fits our provided points
void findBestLine(vector<CvPoint> points, CvPoint* t1, CvPoint* t2)
{
	//Iterate over every possible combination
	double minDist = 999999;
	for (vector<CvPoint>::iterator i = points.begin(); i != points.end(); ++i)
	{
		for (vector<CvPoint>::iterator j = i + 1; j != points.end(); ++j)
		{
//			cout << "Working with lines (" << (*i).x << ", " << (*i).y << ") and (" << (*j).x << ", " << (*j).y << ")\n";
			double dist = 0;
			for (vector<CvPoint>::iterator k = points.begin(); k != points.end(); ++k)
			{
				dist += distToLine(*i, *j, (*k));
			}
			if (dist < minDist && dist < MAX_ERROR)
			{
				minDist = dist;
				*t1 = *i;
				*t2 = *j;
			}
		}
	}
}

//Calculate distance of point to line
double distToLine(CvPoint l1, CvPoint l2, CvPoint pt)
{
	double denom = sqrt(sqr(l2.x - l1.x) + sqr(l2.y - l1.y));
	double num = (pt.x - l1.x) * (l2.x - l1.x) + (pt.y - l1.y) * (l2.y - l1.y);
	return num / denom;
}

void setGlobals()
{
	// mouse click locations (old and new, to tell if we've
	// clicked on a new point
	old_click_pt.x = 0;
	old_click_pt.y = 0;
	new_click_pt.x = 0;
	new_click_pt.y = 0;

	// in theory these should get deleted at the end of the program, too...

	// this is the calibration matrix of the camera
	M = new float[9];
	M[0]=    393.1788940f; M[1]=      0.0000000f; M[2]=    162.8373871f;
	M[3]=      0.0000000f; M[4]=    395.7017517f; M[5]=    121.8078156f;
	M[6]=      0.0000000f; M[7]=      0.0000000f; M[8]=      1.0000000f;

	// these are the distortion parameters of the camera
    D = new float[4];
	D[0]=-0.358375f;
	D[1]=0.168058f;
	D[2]=-0.006497f;
	D[3]=0.001283f;

    // theseimage sizes are hard-coded, but should probably be obtained for each
    // image individually -- however, with our cameras and images, this is safe.
	imageSize.width = 320; imageSize.height = 240;

	// set up some global points for drawing, etc.
    int margin = 12;
	pt1.x = margin; pt1.y = margin; 
    pt2.x = imageSize.width-margin; pt2.y = imageSize.height-margin; 

	pt3.x = imageSize.width/2;
	pt3.y = imageSize.height/2;

	// lower left corner of rectangle
	pt4.x = 0; 
	pt4.y = 0;

	//pt4 and pt5 are set to this because, there is a really wierd crash
	//when they are close to the real bounds. It sometimes breaks, when no red is there
	// or if something obstructs the camera.

	//Update: I found the bug, but did not feel like fixing it
	// It happens when the centroid gets close to the edge and it tries to draw the plus
	// out of bounds.

	// upper right corner of rectangle
	pt5.x = 320;
	pt5.y = 240;

	pt6.x = 92; 
	pt6.y = 24;

	pt7.x = 228;
	pt7.y = 216;

	centroid.x = 160;
	centroid.y = 120;
	pixelCount = 0;
}

void drawMat(IplImage* f_p, int matrix[320][240])
{
	uchar* painted;
	//cvRectangle(f_p, pt4, pt5, CV_RGB(0,0,255), 1); // 1 == thickness

	// This loops through the pixels in the rectangle specified
	// by pt4 and pt5 (see setGlobals for their corners)

	int xlow = (int)mymin(pt4.x,pt5.x);
	int xhi = (int)mymax(pt4.x,pt5.x);
	int ylow = (int)mymin(pt4.y,pt5.y);
	int yhi = (int)mymax(pt4.y,pt5.y);

	pixelCount = 0;
	int xSum = 0;
	int ySum = 0;

	for(int x = xlow; x < xhi; x++)
	{
		for(int y = ylow; y < yhi; y++)
		{
			int realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}
			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			if(matrix[x-xlow][y-ylow] == 1)
			{
				painted[0] = 0;
				painted[1] = 255;
				painted[2] = 255;
				++pixelCount;
				xSum += x;
				ySum += y;
			}
		}
	}

	//Find the largest connected component of the image and return its center

	if(pixelCount != 0)
	{
		findCentroid(matrix, YELLOW, centroid.x, centroid.y, pixelCount);
		
//		centroid.x = xSum/pixelCount;
//		centroid.y = ySum/pixelCount;
		
		cout << "centroid x " << centroid.x << ", centroid y " << centroid.y << endl;

		painted = cvPtr2D(f_p,centroid.y,centroid.x);
		painted[0] = 0;
		painted[1] = 255;
		painted[2] = 0;
		
		/*		for(int i=-3; i <= 3; i++)
		{
			painted = cvPtr2D(f_p,centroid.y + i,centroid.x);
			painted[0] = 0;
			painted[1] = 255;
			painted[2] = 0;
		}
		for(int i=3; i >= -3; i--)
		{
			painted = cvPtr2D(f_p,centroid.y,centroid.x + i);
			painted[0] = 0;
			painted[1] = 255;
			painted[2] = 0;
		}
*/
	}
}

//Find the center of the largest connected blob in the matrix. 
void findCentroid(int matrix[320][240], int color, int& x, int& y, int& pixelCount)
{
	//Initialize all of the pixels to not be in a component.
	for (int i = 0; i < 320; ++i)
		for (int j = 0; j < 240; ++j)
			componentNums[i][j] = 0;
	for (int i = 0; i < MAX_COMPONENTS; ++i)	
	{
		sums[i].x = 0;
		sums[i].y = 0;
		numPoints[i] = 0;
	}


	int numComponents = 1;

	//Iterate over the matrix, summing point locations in a given connected component
	//and reconciling conflicting components.
	int xMax = 320;
	int yMax = 240;
	for (int i = 0; i < xMax; ++i)
	{
		for (int j = 0; j < yMax; ++j)
		{
			if (matrix[i][j] != color)
			{
				componentNums[i][j] = 0;
				continue;
			}
			int component = 0; //Number of component this pixel is in
			int conflictComponent = 0; //Whether or not there is a conflict;
									   //number of other component if so.
			int components[4] = {0, 0, 0, 0}; //Component numbers of four neighbors.

			//Get the component numbers of each of the four neighbors
			components[0] = safeGetComponentNumber(componentNums, i - 1, j);
			components[1] = safeGetComponentNumber(componentNums, i - 1, j - 1);
			components[2] = safeGetComponentNumber(componentNums, i + 1, j - 1);
			components[3] = safeGetComponentNumber(componentNums, i, j - 1);
			//Determine if two nonzero values in components conflict.
			bool foundConflict = false;
			for (int k = 0; k < 4; ++k)
			{
				if (components[k] == 0)
					continue;
				if (foundConflict)
					break;
				component = components[k];
				for (int m = k + 1; m < 4; ++m)
				{
					if (components[m] == 0)
						continue;
					if (components[k] != components[m])
					{
//						printf("Conflict! %d != %d\n", components[k], components[m]);
						component = components[k];
						conflictComponent = components[m];
						foundConflict = true;
						continue;
					}
				}
			}

			if (foundConflict)
			{
//				printf("Reconciling components %d and %d\n", conflictComponent, component);
				reconcileComponents(componentNums, sums, numPoints, numComponents, 
									conflictComponent, component);
			}
			//Create a new component
			else if (component == 0)
			{
//				printf("Creating new component; neighbors %d, %d, %d, %d\n", components[0], components[1], components[2], components[3]);
				componentNums[i][j] = numComponents++;
				if (numComponents >= MAX_COMPONENTS)
				{
					cerr << "Error: more components in image than we can handle\n";
					exit(17);
				}
				CvPoint newP;
				newP.x = i;
				newP.y = j;
				sums[numComponents] = newP;
				numPoints[numComponents] = 1;
			}
			//Add to existing component
			else
			{
//				cerr << "Adding pixel to existing component: (" << i << ", " << j << ")" << endl;
				componentNums[i][j] = component;
				sums[component].x += i;
				sums[component].y += j;
				numPoints[component]++;
			}
		}
	}
	int largest = 0;
	int largestIndex = -1;
	printf("About to find biggest component\n");
	for (int i = 1; i <= numComponents ; ++i)
	{
		if (largest < numPoints[i])
		{
			largest = numPoints[i];
			largestIndex = i;
		}
	}
//	printf("Largest: %d; largestIndex: %d\n", largest, largestIndex); 
	if (largest == 0 || largestIndex == -1)
		return;
	x = sums[largestIndex].x / largest;
	y = sums[largestIndex].y / largest;
	pixelCount = largest;

//	printf("Done with numComponents = %d\n", numComponents);
//	printf("Number of points in each component:\n");
	/*
	for (int i = 1; i <= numComponents; ++i)
	{
		if (numPoints[i] > 0) 
		{
			printf("%d (%d, %d), ", numPoints[i], sums[i].x / numPoints[i], 
			sums[i].y / numPoints[i]);
		}
		else
			printf("%d, ", numPoints[i]);
	}
	printf("\n");
	*/
}

int safeGetComponentNumber(int matrix[320][240], int x, int y)
//int safeGetComponentNumber(apmatrix<int> matrix, int x, int y)
{
	if (x < 0 || y < 0 || x > 319 || y > 239)
		return 0;
	
	return matrix[x][y];
}

CvPoint newCvPoint(int x, int y)
{
	CvPoint n;
	n.x = x;
	n.y = y;
	return n;
}

//Given that two components conflict, make one component equal to the other one.
void reconcileComponents(int matrix[320][240], CvPoint sums[320*240], 
						 int numPoints[320*240], int numComponents, int c1, int c2)
{
	if (c1 == c2) 
	{
		printf("Homebrewed assertion error: tried to reconcile a component with itself");
		exit(169);
	}
		
//	printf("Reconciling components %d (<%d, %d> with %d points) and %d (<%d, %d> with %d points)\n", 
//		c1, sums[c1].x, sums[c1].y, numPoints[c1], c2, sums[c2].x, sums[c2].y, numPoints[c2]);
	sums[c1].x += sums[c2].x;
	sums[c1].y += sums[c2].y;
	numPoints[c1] += numPoints[c2];
	numPoints[c2] = 0;

	//Set all pixels that are in component c2 to be in component c1
	for (int i = 0; i < 320; ++i)
	{
		for (int j = 0; j < 240; ++j)
		{
			if (matrix[i][j] == c2)
				matrix[i][j] = c1;
		}
	}
}

void dilate(int matrix[320][240], int mainColor)
{
	// so far for mainColor,
	// 0 is background, 1 is red, 10 is a marker
	int xMax = 320;
	int yMax = 240;
	for(int x = 0; x < xMax; x++)
	{
		for(int y = 0; y < yMax; y++)
		{
			for(int r = -1; r <= 1; r++)
			{
				for(int c = -1; c <=1; c++)
				{
					if((matrix[x][y] == mainColor) && ((x+r) < xMax) && ((x+r) > 0) && 
						((y+c) < yMax ) && ((y+c) > 0) && !(r==c==0) && 
						(matrix[x+r][y+c] == 0)) 
						// if a color and surroundings in bounds, 
						// and surrounding is not color, then mark it
					{
						matrix[x+r][y+c] = 10; 
						// probably can get rid of all the {}'s
					}
				}
			}
		} // end y
	} // end x
	for(int x = 0; x < xMax; x++)
		for(int y = 0; y < yMax; y++)
			if(matrix[x][y] == 10)
				matrix[x][y] = mainColor; //changes marked ones to mainColor
}
void erode(int matrix[320][240], int mainColor)
{
	// so far for mainColor,
	// 0 is background, 1 is red, 10 is a marker
	int xMax = 320;
	int yMax = 240;
	for(int x = 0; x < xMax; x++)
	{
		for(int y = 0; y < yMax; y++)
		{
			for(int r = -1; r <= 1; r++)
			{
				for(int c = -1; c <=1; c++)
				{
					if((matrix[x][y] == 0) && ((x+r) < xMax) && ((x+r) > 0) && 
						((y+c) < yMax ) && ((y+c) > 0) && !(r==c==0) && 
						(matrix[x+r][y+c] == mainColor)) 
						// if a color and surroundings in bounds, 
						// and surrounding is not color, then mark it
					{
						matrix[x+r][y+c] = 10; 
						// probably can get rid of all the {}'s
					}
				}
			}
		} // end y
	} // end x
	for(int x = 0; x < xMax; x++)
		for(int y = 0; y < yMax; y++)
			if(matrix[x][y] == 10)
				matrix[x][y] = 0; //changes marked ones to mainColor
}